import rospy
import tf.transformations
import tf2_ros
import tf
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped

def doPose(pose):
    # 1.创建发布者坐标相对关系的对象
    pub = tf2_ros.TransformBroadcaster()
    # 2.将pose转换成坐标系相对关系信息
    ts = TransformStamped()
    ts.header.frame_id = "world"
    ts.header.stamp = rospy.Time.now()
    ts.child_frame_id = "turtle1"
    ts.transform.translation.x = pose.x
    ts.transform.translation.y = pose.y
    ts.transform.translation.z = 0.0
    qtn = tf.transformations.quaternion_from_euler(0, 0, pose.theta)
    ts.transform.rotation.x = qtn[0]
    ts.transform.rotation.y = qtn[1]
    ts.transform.rotation.z = qtn[2]
    ts.transform.rotation.w = qtn[3]

    pub.sendTransform(ts)

if __name__ == "__main__":
    rospy.init_node("demo_1")

    sub = rospy.Subscriber("/turtle1/pose", Pose, doPose)

    rospy.spin()